#!/usr/bin/python
# -*- coding: utf-8 -*-



import time


from pyfirmata import Arduino, util




def init_board(servos, usb_port='/dev/ttyUSB1',baudrate=9600):
    print "wait..."
    board = Arduino(usb_port,baudrate=baudrate)
    print "arduino connected!"
    for servo in servos:
        print "init servo %s" % str(servo)
        board.servo_config(servo, min_pulse=544, max_pulse=2400, angle=0)
    return board
    
    



def change_pitch(pitch):
    change(board,11,pitch)


def change_roll(roll):
    change(board,9,roll)


def change_speed(speed):
    change_analog(board,10,speed)


def smooth(value,valueMax=90,valueNull=20):
    if value>valueMax:
        value=valueMax
    if value<valueMax*-1:
        value=valueMax
    if value<valueNull and value>valueNull*-1:
        value=0
    return value



def change_analog(board,servoId,angel,timeout=1/10):
    board.analog[servoId].write(angel)
    time.sleep(timeout)


def change(board,servoId,angel,timeout=1/10):
    board.digital[servoId].write(angel)
    time.sleep(timeout)


def changeservos(board,angle=180,revers=False,servos=(9,11)):
    if revers:
        for ang in reversed(range(angle)):
            for sid in servos:
                change(board,sid, ang)
    else:
        for ang in range(angle):
            for sid in servos:
                change(board,sid, ang)


#servos=(9,11)
#board=init_board(servos)
#for i in range(3):
#    changeservos(board,180,False,servos)
#    changeservos(board,180,True,servos)



